package com.wits.media;

public class ColorConvert {
	
	/**
	 * 
	  	I420: YYYYYYYY UU VV =>YUV420P
		YV12: YYYYYYYY VV UU =>YUV420P
		NV12: YYYYYYYY UVUV =>YUV420SP
		NV21: YYYYYYYY VUVU =>YUV420SP
	 */
	
	static final int RGB2YUV_SHIFT = 8;
	static final int BY = ((int)( 0.098*(1<<RGB2YUV_SHIFT)+0.5));
	static final int BV = ((int)(-0.071*(1<<RGB2YUV_SHIFT)+0.5));
	static final int BU = ((int)( 0.439*(1<<RGB2YUV_SHIFT)+0.5));
	static final int GY = ((int)( 0.504*(1<<RGB2YUV_SHIFT)+0.5));
	static final int GV = ((int)(-0.368*(1<<RGB2YUV_SHIFT)+0.5));
	static final int GU = ((int)(-0.291*(1<<RGB2YUV_SHIFT)+0.5));
	static final int RY = ((int)( 0.257*(1<<RGB2YUV_SHIFT)+0.5));
	static final int RV = ((int)( 0.439*(1<<RGB2YUV_SHIFT)+0.5));
	static final int RU = ((int)(-0.148*(1<<RGB2YUV_SHIFT)+0.5));

	public static int CLIP(int x) {
		if (x > 255) {
			return 255;
		} else if (x < 0) {
			return 0;
		}
		return x;
	}
	
	// RGB -> YUV
	public static int RGB2Y(int r, int g, int b) {
		return CLIP(( (  66 * (r) + 129 * (g) +  25 * (b) + 128) >> 8) +  16);
	}
	
	public static int RGB2U(int r, int g, int b) {
		return CLIP(( ( -38 * (r) -  74 * (g) + 112 * (b) + 128) >> 8) + 128);
	}
	
	public static int RGB2V(int r, int g, int b) {
		return CLIP(( ( 112 * (r) -  94 * (g) -  18 * (b) + 128) >> 8) + 128);
	}
	
	// YUV -> RGB
	public static int C(int Y) {
		return (Y) - 16;
	}
	
	public static int D(int U) {
		return (U) - 128;
	}
	
	public static int E(int V) {
		return (V) - 128;
	}

	public static int YUV2R(int y, int u, int v) {
		return CLIP(( 298 * C(y)              + 409 * E(v) + 128) >> 8);
	}

	public static int YUV2G(int y, int u, int v) {
		return CLIP(( 298 * C(y) - 100 * D(u) - 208 * E(v) + 128) >> 8);
	}
	
	public static int YUV2B(int y, int u, int v) {
		return CLIP(( 298 * C(y) + 516 * D(u)              + 128) >> 8);
	}
	
	/**
	 * 交换YUV420 的UV平面
	 * @param src
	 * @param dst
	 * @param width
	 * @param height
	 */
	public static void swapYUV420p(byte[] src, byte[] dst, int width, int height) {
		final int frameSize = width * height;
		System.arraycopy(src, 0, dst, 0, frameSize);
        System.arraycopy(src, frameSize + frameSize/4, dst, frameSize, frameSize/4);
        System.arraycopy(src, frameSize, dst, frameSize + frameSize/4, frameSize/4);
	}
	
	/// 交换YUV420sp的UV分量
	public static void swapYUV420sp(byte[] src, byte[] dst, int width, int height) {
		final int frameSize = width * height;
		System.arraycopy(src, 0, dst, 0, frameSize);
		for (int i = 0; i < (frameSize/4); i++) {
			dst[i*2] = src[i*2+1];
			dst[i*2+1] = src[i*2];
		}
	}
	
	public static void nv21ToI420(byte[] nv21, byte[] yuv420p, int width, int height) {
		final int frameSize = width * height;
		final int qFrameSize = frameSize / 4;
        System.arraycopy(nv21, 0, yuv420p, 0, frameSize);
        for (int i = 0; i < (qFrameSize); i++) {
            yuv420p[frameSize + i*2] = (nv21[frameSize + qFrameSize + i]);
            yuv420p[frameSize + i*2 + 1] = (nv21[frameSize + i]);
        }
	}
	
	public static void nv21ToYV12(byte[] nv21, byte[] yuv420p, int width, int height) {
		final int frameSize = width * height;
		final int qFrameSize = frameSize / 4;
        System.arraycopy(nv21, 0, yuv420p, 0, frameSize);
        for (int i = 0; i < (qFrameSize); i++) {
            yuv420p[frameSize + i*2] = (nv21[frameSize + i]);
            yuv420p[frameSize + i*2 + 1] = (nv21[frameSize + qFrameSize + i]);
        }
	}
	
	public static void rgb32ToNV21(byte[] argb, byte[] yuv, int width, int height) {
		final int frameSize = width * height;

        int yIndex = 0;
        int uvIndex = frameSize;

        int R, G, B, Y, U, V;
        int index = 0;
        for (int j = 0; j < height; j++) {
            for (int i = 0; i < width; i++) {

            	B = argb[index ++] & 0xff;
            	G = argb[index ++] & 0xff;
            	R = argb[index ++] & 0xff;
            	index ++;
            	
                // well known RGB to YUV algorithm
                Y = ( (  66 * R + 129 * G +  25 * B + 128) >> 8) +  16;
                U = ( ( -38 * R -  74 * G + 112 * B + 128) >> 8) + 128;
                V = ( ( 112 * R -  94 * G -  18 * B + 128) >> 8) + 128;

                // NV21 has a plane of Y and interleaved planes of VU each sampled by a factor of 2
                //    meaning for every 4 Y pixels there are 1 V and 1 U.  Note the sampling is every other
                //    pixel AND every other scanline.
                yuv[yIndex++] = (byte) ((Y < 0) ? 0 : ((Y > 255) ? 255 : Y));
                if (j % 2 == 0 && i % 2 == 0) {
                	yuv[uvIndex++] = (byte)((U<0) ? 0 : ((U > 255) ? 255 : U));
                	yuv[uvIndex++] = (byte)((V<0) ? 0 : ((V > 255) ? 255 : V));
                }

            }
        }
	}
	
	public static void rgb32ToYV12(byte[] argb, byte[] yuv, int width, int height) {
		final int frameSize = width * height;
		
		int yIndex = 0;
        int uIndex = frameSize;
        int vIndex = uIndex + frameSize / 4;

        int R, G, B, Y, U, V;
        int index = 0;
        for (int j = 0; j < height; j++) {
            for (int i = 0; i < width; i++) {

            	B = argb[index ++] & 0xff;
            	G = argb[index ++] & 0xff;
            	R = argb[index ++] & 0xff;
            	// alpha
            	index ++;

                // well known RGB to YUV algorithm
                Y = ( (  66 * R + 129 * G +  25 * B + 128) >> 8) +  16;
                U = ( ( -38 * R -  74 * G + 112 * B + 128) >> 8) + 128;
                V = ( ( 112 * R -  94 * G -  18 * B + 128) >> 8) + 128;

                yuv[yIndex++] = (byte) ((Y < 0) ? 0 : ((Y > 255) ? 255 : Y));
                
                if ((j % 2 == 0) && (i % 2 == 0)) {
                	yuv[uIndex++] = (byte)((U<0) ? 0 : ((U > 255) ? 255 : U));
                	yuv[vIndex++] = (byte)((V<0) ? 0 : ((V > 255) ? 255 : V));
                }
            }
        }
	}
	
	public static void rgb32ToI420(byte[] argb, byte[] yuv, int width, int height) {
		final int frameSize = width * height;
		
		int yIndex = 0;
        int uIndex = frameSize;
        int vIndex = uIndex + frameSize / 4;

        int R, G, B, Y, U, V;
        int index = 0;
        for (int j = 0; j < height; j++) {
            for (int i = 0; i < width; i++) {

            	B = argb[index ++] & 0xff;
            	G = argb[index ++] & 0xff;
            	R = argb[index ++] & 0xff;
            	// alpha
            	index ++;

                // well known RGB to YUV algorithm
                Y = ( (  66 * R + 129 * G +  25 * B + 128) >> 8) +  16;
                U = ( ( -38 * R -  74 * G + 112 * B + 128) >> 8) + 128;
                V = ( ( 112 * R -  94 * G -  18 * B + 128) >> 8) + 128;

                yuv[yIndex++] = (byte) ((Y < 0) ? 0 : ((Y > 255) ? 255 : Y));
                
                if ((j % 2 == 0) && (i % 2 == 0)) {
                	yuv[vIndex++] = (byte)((U<0) ? 0 : ((U > 255) ? 255 : U));
                	yuv[uIndex++] = (byte)((V<0) ? 0 : ((V > 255) ? 255 : V));
                }
            }
        }
	}
	
	public static void yv12ToRGB32(byte[] yuv, int lumStride, int chromStride, byte[] dest, int width, int height) {
		final int frameSize = width * height;
		int yPlane = 0;
		int uPlane = frameSize + frameSize/4;
		int vPlane = frameSize;

	    for (int iRow = 0; iRow < height; iRow++) { 
	        for (int jCol = 0; jCol < width; jCol++) {  
	            int Y = yuv[yPlane + iRow * width + jCol] & 0xFF;  
	            int U = yuv[uPlane + (iRow / 2) * (width / 2) + (jCol / 2)] & 0xFF;  
	            int V = yuv[vPlane + (iRow / 2) * (width / 2) + (jCol / 2)] & 0xFF;  
	
	            int R = YUV2R(Y, U, V);
	            int G = YUV2G(Y, U, V);
	            int B = YUV2B(Y, U, V);
	
	            dest[4 * (iRow * width + jCol) + 0] = (byte)B;  
	            dest[4 * (iRow * width + jCol) + 1] = (byte)G;  
	            dest[4 * (iRow * width + jCol) + 2] = (byte)R;  
	            dest[4 * (iRow * width + jCol) + 3] = 0;  
	        }  
	    } 
	}
	
	public static void i420ToRGB32(byte[] yuv, int lumStride, int chromStride, byte[] dest, int width, int height) {
		final int frameSize = width * height;
		int yPlane = 0;
		int uPlane = frameSize;
		int vPlane = frameSize + frameSize/4;

	    for (int iRow = 0; iRow < height; iRow++) { 
	        for (int jCol = 0; jCol < width; jCol++) {  
	            int Y = yuv[yPlane + iRow * width + jCol] & 0xFF;  
	            int U = yuv[uPlane + (iRow / 2) * (width / 2) + (jCol / 2)] & 0xFF;  
	            int V = yuv[vPlane + (iRow / 2) * (width / 2) + (jCol / 2)] & 0xFF;  
	
	            int R = YUV2R(Y, U, V);
	            int G = YUV2G(Y, U, V);
	            int B = YUV2B(Y, U, V);
	
	            dest[4 * (iRow * width + jCol) + 0] = (byte)B;  
	            dest[4 * (iRow * width + jCol) + 1] = (byte)G;  
	            dest[4 * (iRow * width + jCol) + 2] = (byte)R;  
	            dest[4 * (iRow * width + jCol) + 3] = 0;  
	        }  
	    } 
	}
	
	
}
